#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <memory>
#include <string>

#include "nav2_core/exceptions.hpp"
#include "nav2_custom_planner/nav2_custom_planner.hpp"

namespace nav2_custom_planner{

void CustomPlanner::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,std::string name,std::shared_ptr<tf2_ros::Buffer> tf,std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
    m_tf = tf;
    m_node = parent.lock();
    m_name = name;
    m_costmap = costmap_ros->getCostmap();
    m_global_frame = costmap_ros->getGlobalFrameID();

    nav2_util::declare_parameter_if_not_declared(m_node,m_name+".interpolation_resolution",rclcpp::ParameterValue(0.1));
    m_node->get_parameter(m_name+".interpolation_resolution",m_interpolation_resolution);
}

void CustomPlanner::cleanup(){
    RCLCPP_INFO(m_node->get_logger(),"正在清理类型为CustomPlanner的插件%s",m_name.c_str());
}

void CustomPlanner::activate(){
    RCLCPP_INFO(m_node->get_logger(),"正在激活类型为CustomPlanner的插件%s",m_name.c_str());
}

void CustomPlanner::deactivate(){
    RCLCPP_INFO(m_node->get_logger(),"正在停用类型为CustomPlanner的插件%s",m_name.c_str());
}

nav_msgs::msg::Path CustomPlanner::createPlan(const geometry_msgs::msg::PoseStamped& start,const geometry_msgs::msg::PoseStamped& goal)
{
    nav_msgs::msg::Path global_path;
    global_path.poses.clear();
    global_path.header.stamp = m_node->now();
    global_path.header.frame_id = m_global_frame;
    if(start.header.frame_id != m_global_frame){
        RCLCPP_INFO(m_node->get_logger(),"规划器仅接受来自%s 坐标系的起始位置",m_global_frame.c_str());
        return global_path;
    }

    if(goal.header.frame_id != m_global_frame){
        RCLCPP_INFO(m_node->get_logger(),"规划器仅接受来自%s 坐标系的目标位置",m_global_frame.c_str());
        return global_path;
    }
    
    int total_number_of_loop = std::hypot(goal.pose.position.x - start.pose.position.x,goal.pose.position.y - start.pose.position.y);
    double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop;
    double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop;

    for(int i = 0;i < total_number_of_loop;i++)
    {
        geometry_msgs::msg::PoseStamped pose;
        pose.pose.position.x = start.pose.position.x + x_increment * i;
        pose.pose.position.y = start.pose.position.y + y_increment * i;
        pose.pose.position.z = 0.0;
        pose.header.stamp = m_node->now();
        pose.header.frame_id = m_global_frame;
        global_path.poses.emplace_back(pose);
    }

    for(geometry_msgs::msg::PoseStamped pose : global_path.poses){
        unsigned int mx,my;
        if(m_costmap->worldToMap(pose.pose.position.x,pose.pose.position.y,mx,my)){
            unsigned char cost = m_costmap->getCost(mx,my);
            if(cost == nav2_costmap_2d::LETHAL_OBSTACLE)
            {
                RCLCPP_WARN(m_node->get_logger(),"在(%f,%f)检测到致命障碍物,规划失败。",pose.pose.position.x,pose.pose.position.y);
                throw nav2_core::PlannerException("无法创建目标规划:"+std::to_string(pose.pose.position.x) + std::to_string(pose.pose.position.y));
            }
        }
    }

    geometry_msgs::msg::PoseStamped goal_pose = goal;
    goal_pose.header.stamp = m_node->now();
    goal_pose.header.frame_id = m_global_frame;
    global_path.poses.push_back(goal_pose);
    return global_path;
}


}

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner,nav2_core::GlobalPlanner)